#include "controller_MPC_IPOPT.h"
#include "model_SPDE2D.h"

// ================================================================
// Class: controller_MPC_IPOPT
// ================================================================
controller_MPC_IPOPT::controller_MPC_IPOPT(int iMode, double idt){
  int i_dim[4] = {(iMode+1)*(iMode+1),(iMode+1)*(iMode+1),(iMode+1)*(iMode+1),(iMode+1)*(iMode+1)};
  int o_dim[2] = {1,1};
  block::initialize(4,i_dim,2,o_dim,idt);

  //outputs_next[0][0] = 610;
  //outputs_next[1][0] = 1.0;
  //outputs[0][0] = 610; // Initial temperature, K
  //outputs[1][0] = 1.0; // 

  problem = new MPC_Formulation();
  problem_base = problem;
  solver  = IpoptApplicationFactory();
  
  solver->Options()->SetStringValue("hessian_approximation","limited-memory");
  //	solver->Options()->SetStringValue("derivative_test","none");
  solver->Options()->SetStringValue("derivative_test","first-order");
  status = solver->Initialize();
  if (status != Solve_Succeeded){
    printf("\n\n*** Error during initialization!\n");
    //return (int) status;
  }
  reset();
}
// ------------------------------------------------------------
controller_MPC_IPOPT::~controller_MPC_IPOPT(){
}
// ------------------------------------------------------------
void controller_MPC_IPOPT::reset(){
  time = 0.0;
  problem->reset();
  outputs_next[0][0] = 610;
  outputs_next[1][0] = 1.0;
  outputs[0][0]      = 610; // Initial temperature, K
  outputs[1][0]      = 1.0; // Initial deposition rate, layer/s
}
// ------------------------------------------------------------
void controller_MPC_IPOPT::update(double sysTime){		
  double solution[2];
  while(time<=sysTime){
    //block::output();

    problem->parameterize(time,inputs);
    status = solver->OptimizeTNLP(problem_base);
    
    if(status == Solve_Succeeded){
      // Index iter_count = solver->Statistics()->IterationCount();
      // printf("\n\n*** The problem solved in %d iterations!\n", iter_count);
      
      // Number final_obj = solver->Statistics()->FinalObjective();
      // printf("\n\n*** The final value of the objective function is %e.\n", final_obj);
      
      problem->get_solution(solution);
      outputs[0][0] = solution[0];
      outputs[1][0] = solution[1];
    }		
    time = time+dt;
  }	
}
// ------------------------------------------------------------
/*double cost_fun(double *Inputs){
	double cost = 0.0;
	model_SPDE2D dyn(mode,lattice,dt);
	for(i=1;i<=p;i++)
	{
		dyn.update(i*dt,0.5,Inputs[i-1]);
		R2 = dyn.getR2();
		cost += (R2-R2_set)*(R2-R2_set);
	}

	return cost;
}*/
// ------------------------------------------------------------
